package simunrobot.core;


import java.util.Enumeration;
import java.util.Vector;

import processing.core.*;
import remixlab.proscene.*;
import java.math.*;

public class Robot extends SItem{
	private boolean jacobianinitialized;

	public Robot(SItem parent)
    {
    	super(parent);
    	type = Item.ROBOT;
    	WorldConstraint baseConstraint = new WorldConstraint();
        baseConstraint.setTranslationConstraint(AxisPlaneConstraint.Type.PLANE,new PVector(0.0f,0.0f,1.0f));
        baseConstraint.setRotationConstraint(AxisPlaneConstraint.Type.AXIS,new PVector(0.0f,0.0f,1.0f));
        aframe.setConstraint(baseConstraint);
        this.jacobianinitialized=false;
    }
	public void addEslabon(Eslabon eslabon)
	{
	    childItems.add(eslabon);
	}
	public Eslabon getEslabon(String Name)
	{
	    Enumeration e= childItems.elements();
	    while(e.hasMoreElements())
	    {
	        Eslabon item = (Eslabon)e.nextElement();
	        if (item.getName() == Name)
	            return item;
	    }
	    return null;
	}
	public Eslabon getEslabon(int id)
	{
		Enumeration e= childItems.elements();
	    while(e.hasMoreElements())
	    {
	        Eslabon item = (Eslabon)e.nextElement();
	        if (item.getId() == id)
	            return item;
	    }
	    return null;
	}
	public SItem getEslabonAt(int number)
	{
	    return childItems.elementAt(number);
	}
	public void EslabonMoving(int id)
	{
		Core core;
		core = Core.getInstance();
	    if(core.currentIkType == Core.IKType.CCD)
	    {
	        //Posicion Objetivo
	        Eslabon Objetivo = getEslabon(id);
	        PVector Current = Objetivo.lastPos;
	        PVector Target =  Objetivo.aframe.position();
	        int iter = 2;
	        
	        for (int i = 0 ; i < iter ; i++)
	        {
	        //Obtener ChildNum
	        	int childnum =0;
		        for(int j =0 ; j <childItems.size()  ; j++)
		        {
		        	if(childItems.elementAt(i).getId() == id)childnum=i;
		        }	
	        
	        //Si todavia tiene padres

	        while(childnum--) ////<<<<<<<< esto no sirve en java?
	        {
	            Eslabon item = (Eslabon)childItems.elementAt(childnum);
	            PVector itempos=item.aframe.position();
	            
	            /*float currentdeltaX,currentdeltaY,currentdeltaZ; //Delta al objetivo
	            currentdeltaX = Current.x - itempos.x;
	            currentdeltaY = Current.y - itempos.y;
	            currentdeltaZ = Current.z - itempos.z;
	            float deltaX,deltaY,deltaZ; //Delta al nuevo objetivo
	            deltaX = Target.x-itempos.x;
	            deltaY = Target.y-itempos.y;
	            deltaZ = Target.z-itempos.z;*/

	            PVector currentDelta = PVector.sub(Current, itempos);
	            PVector targetDelta = PVector.sub(Target, itempos);
	            //Magnitudes
	            float magCurr = currentDelta.mag();
	            float magdelta = targetDelta.mag();

	            //Cross
	            PVector cross = currentDelta.cross(targetDelta); 
//	            crossproduct(currentdeltaX,currentdeltaY,currentdeltaZ,deltaX,deltaY,deltaZ,crossX,crossY,crossZ);
	            float dot = currentDelta.dot(targetDelta);
//	            double dot = currentdeltaX*deltaX + currentdeltaY*deltaY +currentdeltaZ*deltaZ;

	            if(!item.thblock || !item.alpblock)
	            {   //Constants
	                float wp = 0;
	                if(magdelta < magCurr && magCurr!=0)
	                    wp = magdelta/magCurr;
	                if(magdelta > magCurr && magdelta!=0)
	                    wp = magCurr/magdelta;
	                PVector AxisZ;
	                PVector LocalZ = new PVector();
	                LocalZ.x=0;
	                LocalZ.y=0;
	                LocalZ.z=1;
	                AxisZ = item.aframe.inverseTransformOf(LocalZ);
	                PVector LocalX = new PVector();
	                LocalX.x=1;
	                LocalX.y=0;
	                LocalX.z=0;
	                if(!item.alpblock)
	                    AxisZ = item.aframe.inverseTransformOf(LocalX);

	                float k1 = wp*AxisZ.z*AxisZ.z*targetDelta.z*currentDelta.z;
	                float k2 = wp*dot;
	                float k3 = wp*cross.z*AxisZ.z-cross.y*AxisZ.y+cross.x*AxisZ.x;
	                //Angulo
	                double Angulo = 0;
	                if(k2 !=0)
	                    Angulo = Math.atan(k3/k2);
	                item.setq(item.getq()+0.3*Angulo);
	            }
	            else
	            {
	            	PVector LocalZ = new PVector();
	                LocalZ.x=0;
	                LocalZ.y=0;
	                LocalZ.z=1;
	                PVector AxisZ = item.aframe.inverseTransformOf(LocalZ);
	                PVector LocalX = new PVector();
	                LocalX.x=1;
	                LocalX.y=0;
	                LocalX.z=0;
	                if(!item.ablock)
	                    AxisZ = item.aframe.inverseTransformOf(LocalX);
	                float k = (targetDelta.z-currentDelta.z)*AxisZ.z+(targetDelta.z-currentDelta.z)*AxisZ.y+(targetDelta.z-currentDelta.z)*AxisZ.x;
	                item.setq(item.getq()+k);
	            }
	        }
	    }
	    }
	}
	String name; /*!< Nombre de la Cadena*/
    String description; /*!< Informacion adicional del Usuario*/
    //SceneItem *selecteditem;
	
}
